﻿/*
 * Copyright (c) 2011
 * Thomas Rozanski, Geoffrey Wright, Zachary Lynn, Jesse Natalie
 * All rights reserved.
 * 
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the team nor the
 *       names of its contributors may be used to endorse or promote products
 *       derived from this software without specific prior written permission.
 * 
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THOMAS ROZANSKI, GEOFFREY WRIGHT,
 * ZACHARY LYNN, OR JESSE NATALIE BE LIABLE FOR ANY
 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

using System;
using System.Collections.Generic;
using Microsoft.Xna.Framework;
using System.Threading.Tasks;

#if RELEASE_TIMER
using System.Diagnostics;
#endif

namespace Kinected
{
    struct Pixel
    {
        public int X, Y;

        public Pixel(int x, int y)
        {
            X = x;
            Y = y;
        }
    }

    class PixelCompare : IEqualityComparer<Pixel>
    {
        public bool Equals(Pixel a, Pixel b)
        {
            return (a.X == b.X && a.Y == b.Y);
        }

        public int GetHashCode(Pixel p)
        {
            return p.X * p.X * p.Y;
        }
    }

    /// <summary>Analyzes the depth image and finds all circles.</summary>
    public class Board
    {
        private const int depthSamples = 520;

        private int windowWidth, windowHeight;

        public bool BoardIsChanging;
        public Timer ParseTimer;

        // Circle data
        private Dictionary<int, Circle> circles;
        private List<Circle> parsedCircles;
        private List<int> addedKeys, removedKeys;
        private int currentKey;

        // 2D matrix for depth image
        private short[,] originalDepthMatrix;
        private short[,] currentDepthMatrix;
        private bool[,] currentBlobs;
        private const int blobStep = 3;
        private int matrixWidth, matrixHeight;

        // Frame changing
        private short[] previousDepthAverages, currentDepthAverages;
        private bool[] frameDifferences;
        private bool boardHasChanged;
        private int frameIndex;

        // background depth
        public short BackgroundDepthValue;
        private bool isBackgroundSet;

        // Random
        private Random random;

        /// <param name="totalCircles">Max number of circles.</param>
        public Board(int totalCircles, int windowWidth, int windowHeight)
        {
            // Initialize the circle
            circles = new Dictionary<int, Circle>(totalCircles);

            this.windowWidth = windowWidth;
            this.windowHeight = windowHeight;

            // Initialize key lists
            currentKey = 0;
            addedKeys = new List<int>();
            removedKeys = new List<int>();

            // Initialize parsed circles
            parsedCircles = new List<Circle>();

            ParseTimer = new Timer();

            random = new Random();
        }

        /// <summary>Prepares the game board.</summary>
        /// <param name="rawDepthImage">RAW depth image.</param>
        public void Initialize(RAWImage rawDepthImage)
        {
            // Initialize the depth image
            matrixWidth = rawDepthImage.Width; matrixHeight = rawDepthImage.Height;
            currentDepthMatrix = new short[matrixHeight, matrixWidth];
            originalDepthMatrix = new short[matrixHeight, matrixWidth];
            currentBlobs = new bool[matrixHeight / blobStep, matrixWidth / blobStep];

            // Frame change detection
            currentDepthAverages = new short[depthSamples];
            previousDepthAverages = new short[depthSamples];

            frameDifferences = new bool[5];
            boardHasChanged = false;
            frameIndex = 0;
        }

        /// <summary>Converts the 1d depth array to an easier to use 2d array.</summary>
        /// <param name="rawDepthImage">RAW depth image.</param>
        private void ConvertDepthArrayToDepthMatrix(RAWImage rawDepthImage)
        {
            int rowSize = matrixWidth * rawDepthImage.Stride;

#if RELEASE_TIMER
            Stopwatch stopWatch = new Stopwatch();
            stopWatch.Start();
#endif

            Parallel.For(0, matrixHeight, y =>
            {
                int currentRow = y * rowSize;

                for (int x = 0; x < matrixWidth; x++)
                {
                    currentDepthMatrix[y, x] = (short)(rawDepthImage.Data[currentRow + x * 2] + (rawDepthImage.Data[currentRow + x * 2 + 1] << 8));
                }
            });

#if RELEASE_TIMER
            stopWatch.Stop();
            Console.WriteLine("Board.ConvertToDepthArray: {0}ms", stopWatch.Elapsed.TotalMilliseconds);
#endif
        }

        #region  Depth Sampling Functions

        /// <summary>Finds the average background depth.</summary>
        private short GetBackgroundDepth()
        {
            int sum = 0;
            int totalSamples = matrixWidth * matrixHeight;

#if RELEASE_TIMER
            Stopwatch stopWatch = new Stopwatch();
            stopWatch.Start();
#endif
            for (int y=0; y < matrixHeight; y++)
            {
                for (int x = 0; x < matrixWidth; x++)
                {
                    sum += currentDepthMatrix[y, x];
                    originalDepthMatrix[y, x] = currentDepthMatrix[y, x];
                }
            }
#if RELEASE_TIMER
            stopWatch.Stop();
            Console.WriteLine("Board.GetBackgroundDepth: {0}ms", stopWatch.Elapsed.TotalMilliseconds);
#endif

            return (short)(sum / totalSamples);
        }


        /// <summary>Takes 352 unique samples of the board and stores them in current depths.</summary>
        private void SampleDepths()
        {
            int index = 0;

#if RELEASE_TIMER
            Stopwatch stopWatch = new Stopwatch();
            stopWatch.Start();
#endif

            for (int y = 0; y < matrixHeight; y+=25)
            {
                for (int x = 0; x < matrixWidth; x+=25)
                {
                    if (currentDepthMatrix[y, x] == 0)
                        index++;
                    else
                    {
                        if (originalDepthMatrix[y, x] == 0)
                            originalDepthMatrix[y, x] = currentDepthMatrix[y, x];
                        currentDepthAverages[index++] = currentDepthMatrix[y, x];
                    }
                }
            }

#if RELEASE_TIMER
            stopWatch.Stop();
            Console.WriteLine("Board.SampleDepths: {0}ms", stopWatch.Elapsed.TotalMilliseconds);
#endif
        }


        /// <summary>Copies the depth averages from current into previous.</summary>
        private void CopyToPrevious()
        {
            for (int i = 0; i < depthSamples; i++)
            {
                previousDepthAverages[i] = currentDepthAverages[i];
            }
        }

        #endregion

        #region Image Comparison Functions

        /// <summary>Decides if the previous image is different from the current image.</summary>
        private bool IsImageDifferent()
        {
            float delta = 0;

            for (int i = 0; i < depthSamples; i++)
            {
                delta += Math.Abs(currentDepthAverages[i] - previousDepthAverages[i]);
            }

            // Store the current value in the buffer
            frameDifferences[frameIndex++] = (delta / depthSamples > 0.5f);

            // Wrap around the buffer index
            if (frameIndex == 5) { frameIndex = 0; }

            for (int i = 0; i < 5; i++)
            {
                if (frameDifferences[i])
                {
                    boardHasChanged = true;
                    return true;
                }
            }

            return false;
        }

        /// <summary>Sees if a pixel has changed (due to movement).</summary>
        private bool NotBackground(int y, int x)
        {
            short difference = (short)(originalDepthMatrix[y, x] - currentDepthMatrix[y, x]);

            return (difference > originalDepthMatrix[y, x] * 0.01f && difference < originalDepthMatrix[y, x] * 0.1f);
        }

        #endregion

        #region Circle List Accessors

        /// <summary>Returns the circle at the specified key.</summary>
        public Circle GetCircle(int key)
        {
            Circle c = circles[key];
            Vector2 FixedCenter = c.Center;
            FixedCenter.X = (FixedCenter.X / matrixWidth) * windowWidth;
            FixedCenter.Y = (FixedCenter.Y / matrixHeight) * windowHeight;

            return new Circle(FixedCenter, c.Height, c.Radius);
        }

        /// <summary>Returns a list of new keys to circles just added to the board.</summary>
        public List<int> AddedKeys()
        {
            return addedKeys;
        }

        /// <summary>Returns a list of keys to circles removed from the board.</summary>
        public List<int> RemovedKeys()
        {
            return removedKeys;
        }

        #endregion

        #region Circle Parsing Functions

        public void ChangeBlobPixels(ref RGBAImage depthImage)
        {
            int stride = depthImage.Stride;
            int rowSize = matrixWidth * stride;
            byte[] data = depthImage.Data;

            int h = matrixHeight / blobStep;
            int w = matrixWidth - matrixWidth % blobStep;
            Parallel.For(0, h, ystep =>
            {
                int y = ystep * blobStep;
                int currentRow = y * rowSize;

                for (int x = 0; x < w; x += blobStep)
                {
                    if (currentBlobs[ystep, x / blobStep])
                    {
                        int index = currentRow + x * stride;
                        data[index] = (byte)~data[index++];
                        data[index] = (byte)~data[index++];
                        data[index] = (byte)~data[index];
                    }
                }
            });
            depthImage.Data = data;
        }

        private Rectangle BlobDetect(int x, int y)
        {
            Queue<Pixel> unTestedPixels = new Queue<Pixel>();
            Dictionary<Pixel, bool> testedPixels = new Dictionary<Pixel, bool>(new PixelCompare());

            // Add the first pixel to the queue
            unTestedPixels.Enqueue(new Pixel(x, y));

            // Bounding rectangle
            int minX = x;
            int maxX = x;

            int minY = y;
            int maxY = y;

            while (unTestedPixels.Count > 0)
            {
                Pixel p = unTestedPixels.Dequeue();

                // Check if the pixel has been visited
                if (testedPixels.ContainsKey(p) || !NotBackground(p.Y, p.X))
                    continue;

                if (IntersectingParsedBlob(p.X, p.Y))
                    continue;
                currentBlobs[p.Y / blobStep, p.X / blobStep] = true;

                // Check the bounds
                if (p.X < 5 || p.X > matrixWidth - 5)
                    continue;
                if (p.Y < 5 || p.Y > matrixHeight - 5)
                    continue;

                // Mark the pixel as tested
                testedPixels.Add(p, false);

                // Adjust bounding rectangle
                if (p.X < minX)
                {
                    minX = p.X;
                }
                else if (p.X > maxX)
                {
                    maxX = p.X;
                }

                if (p.Y < minY)
                {
                    minY = p.Y;
                }
                else if (p.Y > maxY)
                {
                    maxY = p.Y;
                }

                // Compute neighbor pixels
                Pixel left = new Pixel(p.X - blobStep, p.Y);
                Pixel right = new Pixel(p.X + blobStep, p.Y);

                Pixel up = new Pixel(p.X, p.Y - blobStep);
                Pixel down = new Pixel(p.X, p.Y + blobStep);

                // Add them to the queue if they are background pixels
                // and don't already exist
                unTestedPixels.Enqueue(left);
                unTestedPixels.Enqueue(right);
                unTestedPixels.Enqueue(up);
                unTestedPixels.Enqueue(down);
            }

            return new Rectangle(minX, minY, maxX - minX, maxY - minY);
        }

        /// <summary>Determines if the pixel belongs to a parsed circle.</summary>
        private bool IntersectingParsedCircle(int x, int y)
        {
            Vector2 position = new Vector2(x, y);

            // Check for interections between parsed circles
            for (int i = 0; i < parsedCircles.Count; i++)
            {
                if (Circle.Intersection(parsedCircles[i], ref position))
                {
                    return true;
                }
            }

            return false;
        }

        /// <summary>Determines if the pixel belongs to a parsed blob</summary>
        private bool IntersectingParsedBlob(int x, int y)
        {
            return currentBlobs[y / blobStep, x / blobStep];
        }

        /// <summary>Finds the radius of a circle starting from the top.</summary>
        private int FindRadius(int x, int y)
        {
            int diameter = 0;

#if RELEASE_TIMER
            Stopwatch stopWatch = new Stopwatch();
            stopWatch.Start();
#endif

            // Scan down the circle
            for (int i = y; i < matrixHeight; i++)
            {
                if (NotBackground(i, x))
                {
                    diameter++;
                }
                else
                {
                    return diameter / 2;
                }
            }

#if RELEASE_TIMER
            stopWatch.Stop();
            Console.WriteLine("Board.FindRadius: {0}ms", stopWatch.Elapsed.TotalMilliseconds);
#endif

            return 0;
        }

        /// <summary>Determines if the circles are too close.</summary>
        private bool SameCenter(Vector2 centerOne, Vector2 centerTwo)
        {
            return (Math.Abs(centerOne.X - centerTwo.X) < 50
                && Math.Abs(centerOne.Y - centerTwo.Y) < 50);
        }

        /// <summary>Finds circles in the image data.</summary>
        private void ParseCircles()
        {
            // Clear out parsed circles from last update
            parsedCircles.Clear();
            // Clear blob data from last update
            currentBlobs = new bool[matrixHeight / blobStep, matrixWidth / blobStep];

            Vector2 currentPosition = Vector2.Zero;

#if RELEASE_TIMER
            Stopwatch stopWatch = new Stopwatch();
            stopWatch.Start();
#endif

            for (int y = 25; y < matrixHeight - 25; y+=2)
            {
                for (int x = 25; x < matrixWidth - 25; x+=2)
                {
                    //If current position intersects a parsed circle, continue
                    if (IntersectingParsedBlob(x, y))
                        //if (IntersectingParsedCircle(x, y))
                    {
                        continue;
                    }

                    // Potential hit a circle top
                    if (NotBackground(y, x))
                    {
                        Rectangle blob = BlobDetect(x, y);

                        if (blob.Width * blob.Height > 100)
                        {
                            Vector2 center = new Vector2(blob.Center.X, blob.Center.Y);

                            parsedCircles.Add(new Circle(center, currentDepthMatrix[y, x], blob.Width / 1.95f));
                        }
                    }
                }
            }

            // Find which circles already exist
            foreach (KeyValuePair<int, Circle> pair in circles)
            {
                bool circleExist = false;

                for (int i = 0; i < parsedCircles.Count; i++)
                {
                    if (SameCenter(pair.Value.Center, parsedCircles[i].Center))
                    {
                        circleExist = true;
                        parsedCircles.RemoveAt(i--);
                        break;
                    }
                }

                // Mark the circle keys that don't exist
                if (!circleExist)
                {
                    removedKeys.Add(pair.Key);
                }
            }

            // Remove circles that didn't exist
            for (int i = 0; i < removedKeys.Count; i++)
            {
                circles.Remove(removedKeys[i]);
            }

            // Add new circles
            for (int i=0; i < parsedCircles.Count; i++)
            {
                circles.Add(currentKey, parsedCircles[i]);
                addedKeys.Add(currentKey);

                currentKey++;
            }

#if RELEASE_TIMER
            stopWatch.Stop();
            Console.WriteLine("Board.ParseCircles: {0}ms", stopWatch.Elapsed.TotalMilliseconds);
#endif
        }

        #endregion

        /// <summary>Updates the board.</summary>
        /// <param name="gameTime"></param>
        /// <param name="rawDepthImage">RAW depth image.</param>
        /// <param name="haveNewDepthImage">Is there a new depth image?</param>
        public void Update(GameTime gameTime, RAWImage rawDepthImage, bool haveNewDepthImage)
        {
            // Clear added and removed keys from last update
            addedKeys.Clear();
            removedKeys.Clear();

            if (haveNewDepthImage)
            {
#if RELEASE_TIMER
                Stopwatch stopWatch = new Stopwatch();
                stopWatch.Start();
#endif
                ConvertDepthArrayToDepthMatrix(rawDepthImage);

                // Calibrate the camera
                if (!isBackgroundSet)
                {
                    BackgroundDepthValue = GetBackgroundDepth();
                    isBackgroundSet = true;
                }

                SampleDepths();

                BoardIsChanging = IsImageDifferent();

                if (boardHasChanged && !BoardIsChanging)
                {
                    boardHasChanged = false;

                    ParseTimer.Start();

                    ParseCircles();

                    ParseTimer.End();
                }

                CopyToPrevious();

#if RELEASE_TIMER
                stopWatch.Stop();
                Console.WriteLine("Board.Update: {0}ms", stopWatch.Elapsed.TotalMilliseconds);
#endif
            }
        }
    }
}
